package main

import (
	"fmt"
	"iron_man_comm/src/goroslib"
	"iron_man_comm/src/goroslib/pkg/msgs/sensor_msgs"
	"iron_man_comm/src/msg/common_msg"
	"os"
	"time"
)

var gLatestTimestamp uint32
var gNewestOodomTime uint32
var gNewestLdsTime uint32

func writeCSV(delta uint32, pre uint32, curr uint32, id string, mcuTime uint32, dataTime string, currTime string) {
	filePath := "/userdata/odom.csv"
	file, err := os.OpenFile(filePath, os.O_APPEND|os.O_CREATE|os.O_WRONLY, 0644)
	if nil != err {
		fmt.Println("create file fail, err: ", err.Error())
		return
	}
	defer file.Close()

	content := fmt.Sprintf("%s,%s,%d,%d,%d,%d,%s\n", currTime, dataTime, mcuTime, pre, curr, delta, id)
	_, err = file.WriteString(content)
	if err != nil {
		fmt.Println("write file fail, err: ", err.Error())
		return
	}
}

func Abs(n int32) int32 {
	if n < 0 {
		return -n
	}
	return n
}

func writeOdomLdsCSV() {
	filePath := "/userdata/odom_lds.csv"
	file, err := os.OpenFile(filePath, os.O_APPEND|os.O_CREATE|os.O_WRONLY, 0644)
	if nil != err {
		fmt.Println("create file fail, err: ", err.Error())
		return
	}
	defer file.Close()

	delta := int32(gNewestOodomTime - gNewestLdsTime)
	content := fmt.Sprintf("%d,%d,%d\n", gNewestOodomTime, gNewestLdsTime, Abs(delta))
	fmt.Print(content)
	_, err = file.WriteString(content)
	if err != nil {
		fmt.Println("write file fail, err: ", err.Error())
		return
	}
}

func onOdom(msg *common_msg.Odom) {
	gNewestOodomTime = msg.SystemTimeStamp
	currTime := time.Now().Format("2006-01-02 15:04:05.000")
	dataTime := msg.Header.Stamp.Format("2006-01-02 15:04:05.000")
	delta := msg.SystemTimeStamp - gLatestTimestamp //算出和上次时间的差值，单位：0.1ms

	writeCSV(delta, gLatestTimestamp, msg.SystemTimeStamp, msg.Header.FrameId, msg.TimeStamp, dataTime, currTime)
	fmt.Printf("odom, delta: %d, latest: %d, curr: %d, data time: %s, curr time: %s\n",
		delta, gLatestTimestamp, msg.SystemTimeStamp, dataTime, currTime)

	gLatestTimestamp = msg.SystemTimeStamp //更新上次时间
	writeOdomLdsCSV()
}

func onLds(msg *sensor_msgs.LaserScan) {
	gNewestLdsTime = uint32(msg.Header.Stamp.UnixMilli() * 10)
	fmt.Printf("lds, time: %d\n", gNewestLdsTime)
	writeOdomLdsCSV()
}

/**
 * @brief:  odom时间戳测试工具
 */
func main() {
	fmt.Println("start run odom timestamp tool")
	gLatestTimestamp = 0

	node, err := goroslib.NewNode(goroslib.NodeConf{
		MasterAddress: "http://127.0.0.1:11311",
		Name:          "odom_timestamp_test",
	})
	if nil != err {
		fmt.Println("create node fail, err: ", err.Error())
		return
	}
	defer node.Close()
	fmt.Println("create node pass")

	subscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:      node,
		Topic:     "/serial_com_driver/odom",
		Callback:  onOdom,
		QueueSize: 500,
	})
	if err != nil {
		fmt.Println("create subscriber fail, err: ", err.Error())
		return
	}
	defer subscriber.Close()
	fmt.Println("create subscriber pass")

	ldsSubscriber, err := goroslib.NewSubscriber(goroslib.SubscriberConf{
		Node:      node,
		Topic:     "/scan",
		Callback:  onLds,
		QueueSize: 50,
	})
	if err != nil {
		fmt.Println("create ldsSubscriber fail, err: ", err.Error())
		return
	}
	defer ldsSubscriber.Close()
	fmt.Println("create ldsSubscriber pass")

	c := make(chan os.Signal, 1)
	_ = <-c
}
